{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Plot Loop-Closure-Detection\n",
    "\n",
    "Plots statistics on loop closure detection as well as optimized trajectory RPE, APE and trajectory against ground truth."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import yaml\n",
    "import os\n",
    "import copy\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "\n",
    "import logging\n",
    "log = logging.getLogger(__name__)\n",
    "log.setLevel(logging.INFO)\n",
    "if not log.handlers:\n",
    "    ch = logging.StreamHandler()\n",
    "    ch.setLevel(logging.INFO)\n",
    "    ch.setFormatter(logging.Formatter('%(levelname)s - %(message)s'))\n",
    "    log.addHandler(ch)\n",
    "    \n",
    "from evo.tools import file_interface\n",
    "from evo.tools import plot\n",
    "from evo.tools import pandas_bridge\n",
    "\n",
    "from evo.core import sync\n",
    "from evo.core import trajectory\n",
    "from evo.core import metrics\n",
    "from evo.core import transformations\n",
    "from evo.core import lie_algebra as lie\n",
    "\n",
    "%matplotlib inline\n",
    "# %matplotlib notebook\n",
    "import matplotlib.pyplot as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Data Locations\n",
    "\n",
    "Make sure to set the following paths.\n",
    "\n",
    "`vio_output_dir` is the path to the directory containing `output_*.csv` files obtained from logging a run of SparkVio.\n",
    "\n",
    "`gt_data_file` is the absolute path to the `csv` file containing ground truth data for the absolute pose at each timestamp of the dataset."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define directory to VIO output csv files as well as ground truth absolute poses.\n",
    "vio_output_dir = \"/home/sparklab/code/SparkVIO/output_logs/\"\n",
    "gt_data_file = \"/home/sparklab/datasets/EuRoC/mh_04_difficult/mav0/state_groundtruth_estimate0/data.csv\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_ape(data, metric):\n",
    "    \"\"\" Gets APE and APE statistics for two trajectories and a given pose_relation.\n",
    "        \n",
    "        Args:\n",
    "            data:   tuple of trajectories, the first being the reference trajectory\n",
    "                    and the second being the estimated trajectory.\n",
    "            metric: a metrics.PoseRelation instance representing the pose relation\n",
    "                    to use when computing APE.\n",
    "        \n",
    "        Returns:\n",
    "            A metrics.APE instance containing the APE for both trajectories according\n",
    "            to the given metric.\n",
    "    \"\"\"\n",
    "    ape = metrics.APE(metric)\n",
    "    ape.process_data(data)\n",
    "    \n",
    "    return ape\n",
    "\n",
    "\n",
    "def plot_ape(x_axis, ape, size=(18,10), title=None):\n",
    "    \"\"\" Plots APE error against time for a given metrics.APE instance.\n",
    "    \n",
    "        Args:\n",
    "            x_axis: An array-type of values for all the x-axis values (time).\n",
    "            rpe:    A metrics.APE instance with pre-processed data.\n",
    "            size:   A tuple optionally containing the size of the figure to be plotted.\n",
    "    \"\"\"\n",
    "    if title is None:\n",
    "        title = \"APE w.r.t. \" + ape.pose_relation.value\n",
    "    \n",
    "    fig = plt.figure(figsize=size)\n",
    "    plot.error_array(fig, ape.error, x_array=x_axis, statistics=ape.get_all_statistics(), \n",
    "                 name=\"APE\", title=title, xlabel=\"$t$ (s)\")\n",
    "    plt.show()\n",
    "\n",
    "\n",
    "def get_rpe(data, metric):\n",
    "    \"\"\" Gets RPE and RPE statistics for two trajectories and a given pose_relation.\n",
    "        \n",
    "        Args:\n",
    "            data:   tuple of trajectories, the first being the reference trajectory\n",
    "                    and the second being the estimated trajectory.\n",
    "            metric: a metrics.PoseRelation instance representing the pose relation\n",
    "                    to use when computing RPE.\n",
    "        \n",
    "        Returns:\n",
    "            A metrics.RPE instance containing the RPE for both trajectories according\n",
    "            to the given metric.\n",
    "    \"\"\"\n",
    "    # normal mode\n",
    "    delta = 1\n",
    "    delta_unit = metrics.Unit.frames\n",
    "    all_pairs = False\n",
    "\n",
    "    rpe = metrics.RPE(metric, delta, delta_unit, all_pairs)\n",
    "    rpe.process_data(data)\n",
    "    return rpe\n",
    "\n",
    "\n",
    "def plot_rpe(x_axis, rpe, size=(18,10), title=None):\n",
    "    \"\"\" Plots RPE error against time for a given metrics.RPE instance.\n",
    "    \n",
    "        Args:\n",
    "            x_axis: An array-type of values for all the x-axis values (time).\n",
    "            rpe:    A metrics.RPE instance with pre-processed data.\n",
    "            size:   A tuple optionally containing the size of the figure to be plotted.\n",
    "    \"\"\"\n",
    "    if title == None:\n",
    "        title = \"RPE w.r.t. \" + rpe.pose_relation.value\n",
    "    \n",
    "    fig = plt.figure(figsize=size)\n",
    "    plot.error_array(fig, rpe.error, x_array=x_axis, statistics=rpe.get_all_statistics(), \n",
    "                     name=\"RPE\", title=title, xlabel=\"$t$ (s)\")\n",
    "    plt.show()\n",
    "    \n",
    "\n",
    "def downsize_lc_df(df):\n",
    "    \"\"\" Remove all entries from a pandas DataFrame object that have '0' for the timestamp, which\n",
    "        includes all entries that do not have loop closures. Returns this cleaned DataFrame.\n",
    "        \n",
    "        Args:\n",
    "            df: A pandas.DataFrame object representing loop-closure detections, indexed by timestamp.  \n",
    "            \n",
    "        Returns:\n",
    "            A pandas.DataFrame object with only loop closure entries.\n",
    "    \"\"\"\n",
    "    df = df[~df.index.duplicated()]\n",
    "    ts = np.array(df.index.tolist())\n",
    "    good_ts = ts[np.where(ts>0)]\n",
    "    res = df.reindex(index=good_ts)\n",
    "    \n",
    "    return res\n",
    "\n",
    "\n",
    "def convert_abs_traj_to_rel_traj_lcd(df, lcd_df, to_scale=True):\n",
    "    \"\"\" Converts an absolute-pose trajectory to a relative-pose trajectory.\n",
    "    \n",
    "        The incoming DataFrame df is processed element-wise. At each kf timestamp (which is the \n",
    "        index of the DataFrame row) starting from the second (index 1), the relative pose\n",
    "        from the match timestamp to the query stamp is calculated (in the match-\n",
    "        timestamp's coordinate frame). This relative pose is then appended to the \n",
    "        resulting DataFrame.\n",
    "        The resulting DataFrame has timestamp indices corresponding to poses that represent\n",
    "        the relative transformation between the match timestamp and the query one.\n",
    "        \n",
    "        Args:\n",
    "            df: A pandas.DataFrame object with timestamps as indices containing, at a minimum,\n",
    "                columns representing the xyz position and wxyz quaternion-rotation at each\n",
    "                timestamp, corresponding to the absolute pose at that time.\n",
    "            lcd_df: A pandas.DataFrame object with timestamps as indices containing, at a minimum,\n",
    "                columns representing the timestamp of query frames and the timestamps of the \n",
    "                match frames.\n",
    "            to_scale: A boolean. If set to False, relative poses will have their translation\n",
    "                part normalized.\n",
    "        \n",
    "        Returns:\n",
    "            A pandas.DataFrame object with xyz position and wxyz quaternion fields for the \n",
    "            relative pose trajectory corresponding to the absolute one given in 'df', and \n",
    "            relative by the given match and query timestamps.\n",
    "    \"\"\"\n",
    "    rows_list = []\n",
    "    index_list = []\n",
    "    \n",
    "    for i in range(len(lcd_df.index)):\n",
    "        match_ts = lcd_df.timestamp_match[lcd_df.index[i]]\n",
    "        query_ts = lcd_df.timestamp_query[lcd_df.index[i]]\n",
    "        \n",
    "        try:\n",
    "            w_t_bi = np.array([df.at[match_ts, idx] for idx in ['x', 'y', 'z']])\n",
    "            w_q_bi = np.array([df.at[match_ts, idx] for idx in ['qw', 'qx', 'qy', 'qz']])\n",
    "            w_T_bi = transformations.quaternion_matrix(w_q_bi)\n",
    "            w_T_bi[:3,3] = w_t_bi\n",
    "        except:\n",
    "            print \"Failed to convert an abs pose to a rel pose. Timestamp \", \\\n",
    "                  match_ts, \" is not available in ground truth df.\"\n",
    "            continue\n",
    "        \n",
    "        try:\n",
    "            w_t_bidelta = np.array([df.at[query_ts, idx] for idx in ['x', 'y', 'z']])\n",
    "            w_q_bidelta = np.array([df.at[query_ts, idx] for idx in ['qw', 'qx', 'qy', 'qz']])    \n",
    "            w_T_bidelta = transformations.quaternion_matrix(w_q_bidelta)\n",
    "            w_T_bidelta[:3,3] = w_t_bidelta\n",
    "        except:\n",
    "            print \"Failed to convert an abs pose to a rel pose. Timestamp \", \\\n",
    "                  query_ts, \" is not available in ground truth df.\"\n",
    "            continue\n",
    "            \n",
    "        index_list.append(lcd_df.index[i])\n",
    "        \n",
    "        bi_T_bidelta = lie.relative_se3(w_T_bi, w_T_bidelta)\n",
    "        \n",
    "        bi_R_bidelta = copy.deepcopy(bi_T_bidelta)\n",
    "        bi_R_bidelta[:,3] = np.array([0, 0, 0, 1])\n",
    "        bi_q_bidelta = transformations.quaternion_from_matrix(bi_R_bidelta)\n",
    "        bi_t_bidelta = bi_T_bidelta[:3,3]\n",
    "        \n",
    "        if not to_scale:\n",
    "            norm = np.linalg.norm(bi_t_bidelta)\n",
    "            if norm > 1e-6:\n",
    "                bi_t_bidelta = bi_t_bidelta / np.linalg.norm(bi_t_bidelta)\n",
    "        \n",
    "        new_row = {'x': bi_t_bidelta[0], 'y': bi_t_bidelta[1], 'z': bi_t_bidelta[2],\n",
    "                   'qw': bi_q_bidelta[0], 'qx': bi_q_bidelta[1], 'qy': bi_q_bidelta[2],\n",
    "                   'qz': bi_q_bidelta[3],}\n",
    "        rows_list.append(new_row)\n",
    "        \n",
    "    return pd.DataFrame(data=rows_list, index=index_list)\n",
    "\n",
    "\n",
    "def rename_euroc_gt_df(df):\n",
    "    \"\"\" Renames a DataFrame built from a EuRoC ground-truth data csv file to be easier to read.\n",
    "        \n",
    "        Column labels are changed to be more readable and to be identical to the generic pose \n",
    "        trajectory format used with other csv files. Note that '#timestamp' will not actually \n",
    "        be renamed if it is the index of the DataFrame (which it should be). It will be \n",
    "        appropriately renamed if it is the index name.\n",
    "        This operation is 'inplace': It does not return a new DataFrame but simply changes\n",
    "        the existing one.\n",
    "        \n",
    "        Args:\n",
    "            df: A pandas.DataFrame object.\n",
    "    \"\"\"\n",
    "    df.index.names = [\"timestamp\"]\n",
    "    df.rename(columns={\" p_RS_R_x [m]\": \"x\",\n",
    "                       \" p_RS_R_y [m]\": \"y\",\n",
    "                       \" p_RS_R_z [m]\": \"z\",\n",
    "                       \" q_RS_w []\": \"qw\",\n",
    "                       \" q_RS_x []\": \"qx\",\n",
    "                       \" q_RS_y []\": \"qy\",\n",
    "                       \" q_RS_z []\": \"qz\",\n",
    "                       \" v_RS_R_x [m s^-1]\": \"vx\",\n",
    "                       \" v_RS_R_y [m s^-1]\": \"vy\",\n",
    "                       \" v_RS_R_z [m s^-1]\": \"vz\",\n",
    "                       \" b_w_RS_S_x [rad s^-1]\": \"bgx\",\n",
    "                       \" b_w_RS_S_y [rad s^-1]\": \"bgy\",\n",
    "                       \" b_w_RS_S_z [rad s^-1]\": \"bgz\",\n",
    "                       \" b_a_RS_S_x [m s^-2]\": \"bax\",\n",
    "                       \" b_a_RS_S_y [m s^-2]\": \"bay\",\n",
    "                       \" b_a_RS_S_z [m s^-2]\": \"baz\"}, inplace=True)\n",
    "    \n",
    "\n",
    "def rename_lcd_result_df(df):\n",
    "    \"\"\" Renames a DataFrame built from an LCD results measurements csv file to be converted to a trajectory.\n",
    "    \n",
    "        This is an 'inplace' argument and returns nothing.\n",
    "        \n",
    "        Args:\n",
    "            df: A pandas.DataFrame object.\n",
    "    \"\"\"\n",
    "    df.index.names = [\"timestamp\"]\n",
    "    df.rename(columns={\"px\": \"x\",\n",
    "                       \"py\": \"y\",\n",
    "                       \"pz\": \"z\"\n",
    "                      }, inplace=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## LoopClosureDetector Statistics Plotting\n",
    "\n",
    "Gather and plot various statistics on LCD module performance, including RANSAC information, keyframe status (w.r.t. loop closure detection), and loop closure events and the quality of their relative poses."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### LCD Status Frequency Chart\n",
    "\n",
    "Each keyframe is processed for potential loop closures. During this process, the loop-closure detector can either identify a loop closure or not. There are several reasons why a loop closure would not be detected. This plot helps to identify why loop closures are not detected between keyframes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "output_lcd_status_filename = os.path.join(os.path.expandvars(vio_output_dir), \"output_lcd_status.csv\")\n",
    "lcd_debuginfo_df = pd.read_csv(output_lcd_status_filename, sep=',', index_col=0)\n",
    "\n",
    "status_freq_map = {}\n",
    "\n",
    "for status in lcd_debuginfo_df.lcd_status:\n",
    "    if status not in status_freq_map:\n",
    "        status_freq_map[status] = 1\n",
    "    else:\n",
    "        status_freq_map[status] += 1\n",
    "\n",
    "\n",
    "print(\"Full Size of PGO:                       \", lcd_debuginfo_df.pgo_size.tolist()[-1])\n",
    "\n",
    "# Print the overall number of loop closures detected over all time.\n",
    "if \"LOOP_DETECTED\" in status_freq_map:\n",
    "    print(\"Loop Closures Detected:                 \", status_freq_map[\"LOOP_DETECTED\"])\n",
    "else:\n",
    "    print(\"Loop Closures Detected:                 0\")\n",
    "\n",
    "print(\"Loop Closures Registered by PGO by End: \", lcd_debuginfo_df.pgo_lc_count.tolist()[-1])\n",
    "print(\"Loop Closures Accepted by PGO at End:   \", lcd_debuginfo_df.pgo_lc_inliers.tolist()[-1])\n",
    "\n",
    "# Plot failure modes as a histogram.\n",
    "fig = plt.figure(figsize=(18,10))\n",
    "plt.bar(status_freq_map.keys(), status_freq_map.values(), width=1.0)\n",
    "\n",
    "plt.xticks(status_freq_map.keys(), list(status_freq_map.keys()))\n",
    "plt.ylabel('Status Frequency')\n",
    "plt.title('LoopClosureDetector Status Histogram')\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### LCD RANSAC Performance Charts\n",
    "\n",
    "Plot the performance of the geometric-verification and pose-recovery steps. These are handled by Nister (5pt) RANSAC and Arun (3pt) RANSAC respectively.\n",
    "\n",
    "inlier percentages and iterations are plotted for both methods."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "lcd_debuginfo_small_df = downsize_lc_df(lcd_debuginfo_df)\n",
    "\n",
    "#Helper functions for processing data summary.\n",
    "def get_mean(attrib):\n",
    "    ls = lcd_debuginfo_small_df[attrib].tolist()\n",
    "    return float(sum(ls)) / len(ls)\n",
    "\n",
    "def get_min(attrib):\n",
    "    return min(lcd_debuginfo_small_df[attrib])\n",
    "\n",
    "def get_max(attrib):\n",
    "    return max(lcd_debuginfo_small_df[attrib])\n",
    "\n",
    "# Construct and visualize summary. TODO(marcus): use a LaTeX table.\n",
    "summary_stats = [\n",
    "    (\"Average number of mono ransac inliers\", get_mean(\"mono_inliers\")),\n",
    "    (\"Average size of mono ransac input\", get_mean(\"mono_input_size\")),\n",
    "    (\"Average number of stereo ransac inliers\", get_mean(\"stereo_inliers\")),\n",
    "    (\"Average size of stereo ransac input\", get_mean(\"stereo_input_size\")),\n",
    "    (\"Maximum mono ransac iterations\", get_max(\"mono_iters\")),\n",
    "    (\"Maximum stereo ransac iterations\", get_max(\"stereo_iters\")),\n",
    "]\n",
    "\n",
    "attrib_len = [len(attrib[0]) for attrib in summary_stats]\n",
    "max_attrib_len = max(attrib_len)\n",
    "\n",
    "print \"\\nRANSAC Statistic Summary for Loop Closures ONLY:\\n\"\n",
    "for entry in summary_stats:\n",
    "    attrib = entry[0]\n",
    "    value = entry[1]\n",
    "    spacing = max_attrib_len - len(attrib)\n",
    "    print attrib + \" \"*spacing + \": \" + str(value)\n",
    "    \n",
    "    \n",
    "# Plot ransac inlier and iteration statistics.\n",
    "fig1, axes1 = plt.subplots(nrows=1, ncols=2, figsize=(18,10), squeeze=False)\n",
    "\n",
    "lcd_debuginfo_small_df.plot(kind=\"hist\", y=\"mono_inliers\", ax=axes1[0,0])\n",
    "lcd_debuginfo_small_df.plot(kind=\"hist\", y=\"stereo_inliers\", ax=axes1[0,0])\n",
    "lcd_debuginfo_small_df.plot(kind=\"hist\", y=\"mono_iters\", ax=axes1[0,1])\n",
    "lcd_debuginfo_small_df.plot(kind=\"hist\", y=\"stereo_iters\", ax=axes1[0,1])\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### LCD Relative Pose Error Plotting\n",
    "\n",
    "Calculate error statistics for all individual loop closures and plot their error as compared to ground truth. These plots give insight into how reliable the pose determination between two frames is for each loop closure. This pose determination is done via a combination of 5-pt and 3-pt RANSAC matching of the stereo images from the camera."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "gt_df = pd.read_csv(gt_data_file, sep=',', index_col=0)\n",
    "rename_euroc_gt_df(gt_df)\n",
    "\n",
    "output_loop_closures_filename = os.path.join(os.path.expandvars(vio_output_dir), \"output_lcd_result.csv\")\n",
    "output_loop_closures_df = pd.read_csv(output_loop_closures_filename, sep=',', index_col=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "small_lc_df = downsize_lc_df(output_loop_closures_df)\n",
    "rename_lcd_result_df(small_lc_df)\n",
    "gt_rel_df = convert_abs_traj_to_rel_traj_lcd(gt_df, small_lc_df, to_scale=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Convert the gt relative-pose DataFrame to a trajectory object.\n",
    "traj_ref = pandas_bridge.df_to_trajectory(gt_rel_df)\n",
    "\n",
    "# Use the mono ransac file as estimated trajectory.\n",
    "traj_est = pandas_bridge.df_to_trajectory(small_lc_df)\n",
    "traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)\n",
    "\n",
    "print \"traj_ref: \", traj_ref\n",
    "print \"traj_est: \", traj_est"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get RPE for entire relative trajectory.\n",
    "rpe_rot = get_rpe((traj_ref, traj_est), metrics.PoseRelation.rotation_angle_deg)\n",
    "rpe_tran = get_rpe((traj_ref, traj_est), metrics.PoseRelation.translation_part)\n",
    "\n",
    "# Print rotation RPE statistics:\n",
    "rot_summary_stats = [\n",
    "    (\"mean\", rpe_rot.get_statistic(metrics.StatisticsType.mean)),\n",
    "    (\"median\", rpe_rot.get_all_statistics()[\"median\"]),\n",
    "    (\"rmse\", rpe_rot.get_statistic(metrics.StatisticsType.rmse)),\n",
    "    (\"std\", rpe_rot.get_statistic(metrics.StatisticsType.std)),\n",
    "    (\"min\", rpe_rot.get_statistic(metrics.StatisticsType.min)),\n",
    "    (\"max\", rpe_rot.get_statistic(metrics.StatisticsType.max))\n",
    "]\n",
    "\n",
    "attrib_len = [len(attrib[0]) for attrib in rot_summary_stats]\n",
    "max_attrib_len = max(attrib_len)\n",
    "\n",
    "print \"\\nRotation RPE Statistics Summary:\\n\"\n",
    "for entry in rot_summary_stats:\n",
    "    attrib = entry[0]\n",
    "    value = entry[1]\n",
    "    spacing = max_attrib_len - len(attrib)\n",
    "    print attrib + \" \"*spacing + \": \" + str(value)\n",
    "    \n",
    "\n",
    "# Print translation RPE statistics:\n",
    "tram_summary_stats = [\n",
    "    (\"mean\", rpe_tran.get_statistic(metrics.StatisticsType.mean)),\n",
    "    (\"median\", rpe_tran.get_all_statistics()[\"median\"]),\n",
    "    (\"rmse\", rpe_tran.get_statistic(metrics.StatisticsType.rmse)),\n",
    "    (\"std\", rpe_tran.get_statistic(metrics.StatisticsType.std)),\n",
    "    (\"min\", rpe_tran.get_statistic(metrics.StatisticsType.min)),\n",
    "    (\"max\", rpe_tran.get_statistic(metrics.StatisticsType.max))\n",
    "]\n",
    "\n",
    "attrib_len = [len(attrib[0]) for attrib in tram_summary_stats]\n",
    "max_attrib_len = max(attrib_len)\n",
    "\n",
    "print \"\\nTranslation RPE Statistics Summary:\\n\"\n",
    "for entry in tram_summary_stats:\n",
    "    attrib = entry[0]\n",
    "    value = entry[1]\n",
    "    spacing = max_attrib_len - len(attrib)\n",
    "    print attrib + \" \"*spacing + \": \" + str(value)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## LoopClosureDetector PGO-Optimized Trajectory Plotting\n",
    "\n",
    "Plot the APE, RPE, and trajectory of the Pose-graph-optimized trajectory, including loop closures on top of regular odometry updates.\n",
    "\n",
    "The results are visualized against both ground truth and the odometry-estimate alone to show the performance gain from loop closure detection."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load ground truth and estimated data as csv DataFrames.\n",
    "gt_df = pd.read_csv(gt_data_file, sep=',', index_col=0)\n",
    "\n",
    "output_poses_filename = os.path.join(os.path.expandvars(vio_output_dir), \"output_posesVIO.csv\")\n",
    "output_poses_df = pd.read_csv(output_poses_filename, sep=',', index_col=0)\n",
    "\n",
    "output_pgo_poses_filename = os.path.join(os.path.expandvars(vio_output_dir), \"output_lcd_optimized_traj.csv\")\n",
    "output_pgo_poses_df = pd.read_csv(output_pgo_poses_filename, sep=',', index_col=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "gt_df = gt_df[~gt_df.index.duplicated()]\n",
    "\n",
    "rename_euroc_gt_df(gt_df)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Convert the gt relative-pose DataFrame to a trajectory object.\n",
    "traj_ref = pandas_bridge.df_to_trajectory(gt_df)\n",
    "\n",
    "# Compare against the VIO without PGO.\n",
    "traj_ref_cp = copy.deepcopy(traj_ref)\n",
    "traj_vio = pandas_bridge.df_to_trajectory(output_poses_df)\n",
    "traj_ref_cp, traj_vio = sync.associate_trajectories(traj_ref_cp, traj_vio)\n",
    "traj_vio = trajectory.align_trajectory(traj_vio, traj_ref_cp, correct_scale=False,\n",
    "                                       discard_n_start_poses = int(discard_n_start_poses),\n",
    "                                       discard_n_end_poses = int(discard_n_end_poses))\n",
    "\n",
    "# Use the PGO output as estimated trajectory.\n",
    "traj_est = pandas_bridge.df_to_trajectory(output_pgo_poses_df)\n",
    "\n",
    "# Associate the data.\n",
    "traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)\n",
    "traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False,\n",
    "                                       discard_n_start_poses = int(discard_n_start_poses),\n",
    "                                       discard_n_end_poses = int(discard_n_end_poses))\n",
    "\n",
    "print \"traj_ref: \", traj_ref\n",
    "print \"traj_vio: \", traj_vio\n",
    "print \"traj_est: \", traj_est"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Absolute-Pose-Error Plotting\n",
    "\n",
    "Plot absolute-pose-error along the entire trajectory. APE gives a good sense of overall VIO performance across the entire trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot APE of trajectory rotation and translation parts.\n",
    "num_of_poses = traj_est.num_poses\n",
    "traj_est.reduce_to_ids(range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1))\n",
    "traj_ref.reduce_to_ids(range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1))\n",
    "traj_vio.reduce_to_ids(range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1))\n",
    "\n",
    "seconds_from_start = [t - traj_est.timestamps[0] for t in traj_est.timestamps]\n",
    "\n",
    "ape_tran = get_ape((traj_ref, traj_est), metrics.PoseRelation.translation_part)\n",
    "plot_ape(seconds_from_start, ape_tran, title=\"VIO+PGO ATE in Meters\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the ground truth and estimated trajectories against each other with APE overlaid.\n",
    "\n",
    "plot_mode = plot.PlotMode.xy\n",
    "fig = plt.figure(figsize=(18,10))\n",
    "ax = plot.prepare_axis(fig, plot_mode)\n",
    "plot.traj(ax, plot_mode, traj_ref, '--', \"gray\", \"reference\")\n",
    "plot.traj(ax, plot_mode, traj_vio, '.', \"gray\", \"vio without pgo\")\n",
    "plot.traj_colormap(ax, traj_est, ape_tran.error, plot_mode,\n",
    "                   min_map=ape_tran.get_all_statistics()[\"min\"],\n",
    "                   max_map=ape_tran.get_all_statistics()[\"max\"],\n",
    "                   title=\"VIO+PGO Trajectory Tracking - Color Coded by ATE\")\n",
    "ax.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Relative-Pose-Error Plotting\n",
    "\n",
    "Plot relative-pose-error along the entire trajectory. RPE gives a good sense of overall VIO performance from one frame to the next."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Get RPE for entire relative trajectory.\n",
    "rpe_rot = get_rpe((traj_ref, traj_est), metrics.PoseRelation.rotation_angle_deg)\n",
    "rpe_tran = get_rpe((traj_ref, traj_est), metrics.PoseRelation.translation_part)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot RPE of trajectory rotation and translation parts.\n",
    "seconds_from_start = [t - traj_est.timestamps[0] for t in traj_est.timestamps[1:]]\n",
    "\n",
    "plot_rpe(seconds_from_start, rpe_rot, title=\"VIO+PGO RRE in Degrees\")\n",
    "plot_rpe(seconds_from_start, rpe_tran, title=\"VIO+PGO RTE in Meters\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# important: restrict data to delta ids for plot.\n",
    "traj_ref_plot = copy.deepcopy(traj_ref)\n",
    "traj_est_plot = copy.deepcopy(traj_est)\n",
    "traj_ref_plot.reduce_to_ids(rpe_rot.delta_ids)\n",
    "traj_est_plot.reduce_to_ids(rpe_rot.delta_ids)\n",
    "\n",
    "# Plot the ground truth and estimated trajectories against each other with RPE overlaid.\n",
    "\n",
    "plot_mode = plot.PlotMode.xy\n",
    "fig = plt.figure(figsize=(18,10))\n",
    "ax = plot.prepare_axis(fig, plot_mode)\n",
    "plot.traj(ax, plot_mode, traj_ref_plot, '--', \"gray\", \"reference\")\n",
    "plot.traj_colormap(ax, traj_est_plot, rpe_rot.error, plot_mode,\n",
    "                   min_map=rpe_rot.get_all_statistics()[\"min\"],\n",
    "                   max_map=rpe_rot.get_all_statistics()[\"max\"],\n",
    "                   title=\"VIO+PGO Trajectory Tracking - Color Coded by RRE\")\n",
    "ax.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_vio = pandas_bridge.df_to_trajectory(output_poses_df)\n",
    "traj_ref, traj_vio = sync.associate_trajectories(traj_ref, traj_est)\n",
    "traj_vio = trajectory.align_trajectory(traj_vio, traj_ref, correct_scale=False)\n",
    "\n",
    "# Plot the trajectories for quick error visualization.\n",
    "\n",
    "fig = plt.figure(figsize=(18,10))\n",
    "traj_by_label = {\n",
    "    \"VIO only\": traj_vio,\n",
    "    \"VIO + PGO\": traj_est,\n",
    "    \"reference\": traj_ref\n",
    "}\n",
    "plot.trajectories(fig, traj_by_label, plot.PlotMode.xyz, title=\"PIM Trajectory Tracking in 3D\")\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.15+"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
